#include "GlobalVar.h"
#include "BasicFun.h"
#include "FindLine.h"
#include "FillSpecialLine.h"
#include "MainProcess.h"
#include "CircleIsland.h"
//#include "FirstLineProcess.h"
#include "canny.h"
#include "SpecialElem.h"
#include "Forks.h"

#define CIRCLE 2
//================================================================//
//  @brief  :		初始化参数
//  @param  :		void
//  @return :		void(ML_Count/ErrorFLag/g_RoadType/LeftIntLine/RightIntLine/LPnt/RPnt/LL/RL/ML)
//  @note   :		void
//================================================================//
void VarInit(void)
{
	ML_Count = 0;
	ErrorFlag = 0;
	g_RoadType = 0;
	LeftIntLine = 0;
	RightIntLine = 0;
	//下面某些地方起了关键作用
	LeftPnt.ErrCol = RightPnt.ErrCol = 0;
	LeftPnt.ErrRow = RightPnt.ErrRow = DOWN_EDGE;
	for (int i = 0; i < IMG_ROW; i++)
	{
		LL[i] = LEFT_EDGE;
		RL[i] = RIGHT_EDGE;
		ML[i] = MID;
	}

}

//================================================================//
//  @brief  :		普通补图主程序
//  @param  :		void
//  @return :		void
//  @note   :		void
//================================================================//
void MainFill(void)
{
	VarInit();
	FirstLineV4();
	g_RoadType = FirstRowProcess();

	if (0 == g_RoadType)
	{
		FindLineNormal(1);
//		ImgJudgeStopLine();		//识别停车
		// ImgJudgeObstacle();     //识别坡道路障直道断路					
		// ImgJudgeCurveBroken();	//弯道断路
		// if (Img_GrayJumpOpen)
		// {
		// 	if (!Img_SpecialElemFlag && 1 == ImgJudgeOutBroken())
		// 	{
		// 		Img_BrokenFlag = 2;
		// 		Img_SpecialElemFlag = 1;
		// 	}
		// 	//ImgJudgeOutBroken();			//灰度跳变检测断路
		// }

#if CIRCLE == 2
		//CircleFlag = ImgJudgeCircle(0);
		CircleFlag = Img_JudgeCircleIsland(0);
		if (CL == CircleFlag)
		{
			int tmp_row = LeftPnt.ErrRow;
			GetPointA();
			GetPointB();
			GetPointC();
			GetPointD();
			FillLineAB();
			FillLineCD();
			FillAllEdge();
//			if (Ind_CircleOpen)
//				CircleFlag = CN;
//			else
//			{
				if (tmp_row > DOWN_EDGE - 10)
					;
				else CircleFlag = CN;
//			}
		}
		else if (CR == CircleFlag)
		{
			int tmp_row = RightPnt.ErrRow;
			GetPointA();
			GetPointB();
			GetPointC();
			GetPointD();
			FillLineAB();
			FillLineCD();
			FillAllEdge();
//			if (Ind_CircleOpen)
//				CircleFlag = CN;
//			else
//			{
				if (tmp_row > DOWN_EDGE - 10)
					;
				else CircleFlag = CN;
//			}
		}
		else
#endif // CIRCLE
		{
			ForksHandle();
			if(Forks_State&&Forks_State!=3&&Forks_State!=5)
			{
				return ;
			}
			if (LeftPnt.Type == 2 && RightPnt.Type == 2)		//十字补图
			{
				if (LeftPnt.ErrRow - RightPnt.ErrRow > 10 || RightPnt.ErrRow - LeftPnt.ErrRow > 10)
					FillBevelCross();
				else
					FillLevelCross();
				FindLineNormal(0);

			}

		}
			
	}
	if (1 == g_RoadType)
	{
		FindLineLost();
#if CIRCLE == 2
		//CircleFlag = ImgJudgeCircle(1);
		CircleFlag = Img_JudgeCircleIsland(1);
		if (CL == CircleFlag)
		{
			GetPointA();
			GetPointB();
			GetPointC();
			GetPointD();
			FillLineAB();
			FillLineCD();
			FillAllEdge();
//			if (Ind_CircleOpen)
//				CircleFlag = CN;
		}
		/*else*/ if (CR == CircleFlag)
		{
			GetPointA();
			GetPointB();
			GetPointC();
			GetPointD();
			FillLineAB();
			FillLineCD();
			FillAllEdge();
//			if (Ind_CircleOpen)
//				CircleFlag = CN;
		}
		else
#endif
		{
			ForksHandle();
			if(Forks_State&&Forks_State!=3&&Forks_State!=5)
			{
				return ;
			}
			if (1 == g_RoadType && 2 == LeftPnt.Type && 2 == RightPnt.Type)
			{
				FillBevelCross();
				FindLineNormal(0);
			}
		}
			
	}
	if (2 == g_RoadType)
	{
		ForksHandle();
		if(Forks_State&&Forks_State!=3&&Forks_State!=5)
		{
			return ;
		}
		FillFourCross();
		FindLineNormal(0);
	}
	FillMiddleLine();
}

//================================================================//
//  @brief  :		期望速度控制
//  @param  :		middle 车辆中值
//  @return :		void
//  @note   :		void
//================================================================//
int GetSpeedRow(int middle, int left_row, int right_row)
{
	Point P[3] = {
		{DOWN_EDGE, middle - 5},
	{DOWN_EDGE, middle},
	{DOWN_EDGE, middle + 5} };
	int min_row = MIN(left_row, right_row);
	for (int i = DOWN_EDGE; i > UP_EDGE; i--)
	{
		if ((LL[i] - P[0].Col) * (LL[i - 1] - P[0].Col) <= 0
			|| (RL[i] - P[0].Col) * (RL[i - 1] - P[0].Col) <= 0
			|| UP_EDGE + 1 == i)
		{
			P[0].Row = MAX(i, min_row);
			break;
		}
	}
	for (int i = DOWN_EDGE; i > UP_EDGE; i--)
	{
		if ((LL[i] - P[1].Col) * (LL[i - 1] - P[1].Col) <= 0
			|| (RL[i] - P[1].Col) * (RL[i - 1] - P[1].Col) <= 0
			|| UP_EDGE + 1 == i)
		{
			P[1].Row = MAX(i, min_row);
			break;
		}
	}
	for (int i = DOWN_EDGE; i > UP_EDGE; i--)
	{
		if ((LL[i] - P[2].Col) * (LL[i - 1] - P[2].Col) <= 0
			|| (RL[i] - P[2].Col) * (RL[i - 1] - P[2].Col) <= 0
			|| UP_EDGE + 1 == i)
		{
			P[2].Row = MAX(i, min_row);
			break;
		}
	}
	return (P[1].Row + P[0].Row + P[2].Row) / 3;
}


//================================================================//
//  @brief  :		补图主程序
//  @param  :		void
//  @return :		void
//  @note   :		void
//================================================================//
void GetML(void)
{
	CannyEdge();
#if CIRCLE
	if (CircleFlag)		//is CircleIsland 
	{
		CircleFill();
	}
	else
#endif // CIRCLE
	{
		// if (Img_SpecialElemFlag)
		// 	SpecialElemFill();
		// if (!Img_SpecialElemFlag)
			MainFill();
	}

	//中线校验
	if ((RL[DOWN_EDGE] - LL[DOWN_EDGE] <= 40 || ML_Count > DOWN_EDGE - 20		//下边界过小，有效行数过低
		|| RightPnt.ErrCol - LeftPnt.ErrCol > 100)&&(!Forks_State))							//上边界不收敛
	{
		ErrorFlag = 4;
	}
	if (!ErrorFlag)
		SpeedRow = GetSpeedRow(ControlMid, LeftPnt.ErrRow, RightPnt.ErrRow);
	// if (!ErrorFlag && UP_EAGE + 1 >= SpeedRow)
	// 	Ind_LongRoadFlag = 1;
	// else Ind_LongRoadFlag = 0;
}

//================================================================//
//  @brief  :		停车保护
//  @param  :		void
//  @return :		void
//  @note   :		void
//================================================================//
void Img_ProtectStop()
{
	int sum=0;
	for(int i=0;i<188;i++)
		sum+=image[DOWN_EDGE][i];
	if(sum/188<60)
		g_drive_flag=0;
}